"""
This is part of Human Robot Dynamics-Control, By Tourists.

Based on "OpenLoong Dynamics Control"
Transfer from mujoco to ISAAC SIM by IsaacLab

"""
from math import fabs

class RampTrajectory:
    def __init__(self):
        self.dt = 0.0
        self.y = 0.0
        self.yDes = 0.0
        
        self._yOld = 0.0
        self._k = 0.0

    def setPara(self, yDesIn, timeToReach):
        self.yDes = yDesIn
        if timeToReach < 0.001:
            timeToReach=0.001
        self._k = (yDesIn - self._yOld) / timeToReach

    def set_para_dirt(self, y_des_in, delta_y):
        self.y_des = y_des_in
        self.k = delta_y / self.dt

    def step(self):
        self.y = self._yOld + self._k * self.dt
        if fabs(self.yDes - self.y) < fabs(1.5 * self._k * self.dt):
            self.y = self.yDes
        self._yOld = self.y
        return self.y

    def is_reach_des(self):
        if abs(self.yDes - self.y) < abs(self.k * self.dt):
            return True
        else:
            return False

    def reset_out(self, yOut):
        self.y = yOut
        self.yOld = yOut

